//
// Created by fins on 24-10-6.
//

#include "pointcloud/filter.hpp"
#include "pointcloud/filter_custom.hpp"


Filter::Filter()
    : Node("pointcloud_filter_node"),
      tf_buffer_(this->get_clock()),
      tf_listener_(tf_buffer_) {

    config = YAML::LoadFile(YAML_FILE_PATH);
    origin_cloud = config["input_topic"]["points_cloud"].as<std::string>();
    processed_cloud = config["output_topic"]["processed_cloud"].as<std::string>();
    frame_id_robot = config["frame_id"]["robot"].as<std::string>();
    frame_id_lidar = config["frame_id"]["lidar"].as<std::string>();
    use_box_filter = config["box_filter"]["use_box_filter"].as<bool>();
    transform_to_robot_frame = config["transform_to_robot_frame"].as<bool>();

    point_cloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
            origin_cloud , 10, std::bind(&Filter::pointcloud_callback, this, std::placeholders::_1));

    processed_point_cloud_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(processed_cloud, 10);

    while (not transform_initialized_) {
        try {
            transform_l2r = tf_buffer_.lookupTransform(frame_id_robot, frame_id_lidar, tf2::TimePointZero);
            transform_r2l = tf_buffer_.lookupTransform(frame_id_lidar, frame_id_robot, tf2::TimePointZero);
            transform_initialized_ = true;
        } catch (tf2::TransformException &ex) {
            RCLCPP_WARN(this->get_logger(), "%s", ex.what()); // 如果无法找到变换，返回
            transform_initialized_ = false;
            rclcpp::sleep_for(std::chrono::milliseconds(100));
        }
    }
    // 初始化 cloud_filter
    cloud_filter.loadParams(YAML_FILE_PATH);
    cloud_filter.set_transform_r2l(transform_r2l);
}


void Filter::pointcloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {

    auto start_time = std::chrono::high_resolution_clock::now();

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*msg, *cloud);

    RCLCPP_INFO(this->get_logger(), "point cloud size: %zu", cloud->size());
    pcl::PointCloud<pcl::PointXYZ>::Ptr processed_cloud(new pcl::PointCloud<pcl::PointXYZ>);

    // 获取变换矩阵
    if (transform_to_robot_frame){
    for (auto &point : cloud->points) {
        // 将点从 lidar_frame 转换到 robot_base
        geometry_msgs::msg::PointStamped point_stamped;
        point_stamped.header.frame_id = frame_id_lidar;  // 设置原始帧
        point_stamped.header.stamp = msg->header.stamp;  // 使用原始点云的时间戳
        point_stamped.point.x = point.x;
        point_stamped.point.y = point.y;
        point_stamped.point.z = point.z;

        geometry_msgs::msg::PointStamped transformed_point;
        tf2::doTransform(point_stamped, transformed_point, transform_l2r);  // 使用 doTransform 将点从 lidar_frame 转换到 robot_base

        point.x = transformed_point.point.x;
        point.y = transformed_point.point.y;
        point.z = transformed_point.point.z;
    }}

    // 使用 box_filter 过滤点云
    if (use_box_filter) {
        cloud_filter.filter(cloud, processed_cloud);
    } else {
        *processed_cloud = *cloud;
    }

    RCLCPP_INFO(this->get_logger(), "processed point cloud size: %zu", processed_cloud->size());
    sensor_msgs::msg::PointCloud2::SharedPtr output_msg(new sensor_msgs::msg::PointCloud2);
    pcl::toROSMsg(*processed_cloud, *output_msg);
    // 设置header
    if (transform_to_robot_frame) {
        output_msg->header.frame_id = frame_id_robot;
    } else {
        output_msg->header.frame_id = frame_id_lidar;
    }
    processed_point_cloud_pub_->publish(*output_msg);
    // 获取结束时间
    auto end_time = std::chrono::high_resolution_clock::now();

    // 计算总运行时间
    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time).count();
    RCLCPP_INFO(this->get_logger(), "Point cloud processing took %ld milliseconds", duration);
}


int main(int argc, char **argv) {
    YAML::Node config = YAML::LoadFile(YAML_FILE_PATH);
    // 根据点云的类型选择不同的过滤器
    std::string point_type = config["point_type"].as<std::string>();
    if (point_type == "pcl") {
        rclcpp::init(argc, argv);
        rclcpp::spin(std::make_shared<Filter>());
        rclcpp::shutdown();
    } else if (point_type == "custom") {
        rclcpp::init(argc, argv);
        rclcpp::spin(std::make_shared<Filter_Custom>());
        rclcpp::shutdown();
    } else {
        RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Invalid point type");
        return -1;
    }
    return 0;
}